#!/usr/bin/env python3
# coding:utf-8

import rospy
from get_path.srv import *
from sensor_msgs.msg import NavSatFix
from Global_planner import GaodePlanner   # 你之前写的类封装

class PathServer:
    def __init__(self, ):
        """
        初始化 PathServer
        :param api_key: 高德 API key
        """
        rospy.init_node("path_server", anonymous=True)

        # 高德路径规划器
        self.api_key = "f4cb8aadaa299f5d605fa8ecd641bbaa"
        self.planner = GaodePlanner(self.api_key)

        # 注册服务
        self.service = rospy.Service("path_get_service", Pathget, self.handle_function)
        rospy.loginfo("✅ Path server is ready: /path_get_service")

    def handle_function(self, req):
        """
        服务回调函数：输入起点、终点（GCJ02, [lat, lon]），返回路径点列表（NavSatFix[]）
        """
        origin = req.origin_point
        destination = req.destination_point

        rospy.loginfo("Request: origin=(%.6f, %.6f), destination=(%.6f, %.6f)",
                      origin[0], origin[1], destination[0], destination[1])

        # 调用高德 API 获取路径 (GCJ02)
        result_path = self.planner.get_path(origin, destination, debug=False)

        fix_path = []
        for point in result_path:
            fix_point = NavSatFix()
            fix_point.latitude = point[0]   # lat
            fix_point.longitude = point[1]  # lon
            fix_point.altitude = 0.0        # 没有高程信息，填 0
            fix_path.append(fix_point)

        point_num = len(fix_path)
        rospy.loginfo("Returning %d points", point_num)

        return PathgetResponse(fix_path, point_num)

    def spin(self):
        rospy.spin()


if __name__ == "__main__":
    # ⚠️ 注意：这里要换成你自己申请的 API key
    server = PathServer()
    server.spin()
